package jp.seraph.robocup.soccer.research.ikinematics;

import org.apache.commons.math.geometry.Rotation;
import org.apache.commons.math.geometry.Vector3D;

import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.math.Point3D;

public class LineModel {
    public LineModel(LineModel aParent, int aLength) {
        mParent = aParent;
        mLength = aLength;
        mRelativeAngle = Angle.ZERO;
        mAngleVelocity = Angle.ZERO;
        mTranslation = new Vector3D(aLength, 0, 0);
    }

    private LineModel mParent;
    private int mLength;
    private Angle mRelativeAngle;
    private Vector3D mAxis = Vector3D.plusK;
    private Vector3D mTranslation;
    private Angle mAngleVelocity;

    public Point3D getStartPoint() {
        if (this.isRoot())
            return Point3D.origin;
        else
            return mParent.getEndPoint();
    }

    public Point3D getEndPoint() {
        return this.getCoordinateAsRoot(new Point3D(mTranslation));
    }

    public double getLength() {
        return mLength;
    }

    public Angle getAngle(){
        return mRelativeAngle;
    }

    public Angle getAngleVelocity(){
        return mAngleVelocity;
    }

    public void setAngle(Angle aAngle) {
        mAngleVelocity = aAngle.subtract(mRelativeAngle);
        mRelativeAngle = aAngle;
    }

    public Point3D getCoordinateAsRoot(Point3D aLocalPoint) {

        // この座標系原点(StartPoint)からaLocalPointへのベクトルを作る
        Vector3D tLotatedVector = new Vector3D(aLocalPoint.getX(), aLocalPoint.getY(), aLocalPoint.getZ());

        // Angleで回転させる
        Rotation tRotation = new Rotation(mAxis, mRelativeAngle.getRad());
        tLotatedVector = tRotation.applyTo(tLotatedVector);
        double tLength = tLotatedVector.getNorm();
        if (Double.isNaN(tLength))
            tLotatedVector = Vector3D.zero;

        // 親オブジェクトから見たaLocalPoint座標は
        // 親オブジェクトからこのジョイントへの平行移動に回転させたベクトルを足したもの

        if (this.isRoot())
            return new Point3D(tLotatedVector);
        else {
            Point3D tRerativePoint = new Point3D(mParent.getLength(), 0, 0).add(tLotatedVector);
            return mParent.getCoordinateAsRoot(tRerativePoint);
        }
    }

    public boolean setEndPoint(Point3D aPoint) {
        if (this.isRoot()) {
            return setEndPointAsRoot(aPoint);
        }

        Point3D tFirstPoint = mParent.getStartPoint();
        double tDistance = tFirstPoint.getDistance(aPoint);
        double tMaxDistance = mParent.getLength() + this.getLength();
        double tMinDistance = Math.abs(mParent.getLength() - this.getLength());
        Vector3D tStartToEnd = aPoint.subtract(tFirstPoint);
        double tTargetRootAngle = Math.atan2(aPoint.getY(), aPoint.getX());
        double tTargetParentAngle = Math.atan2(tStartToEnd.getY(), tStartToEnd.getX());

        // Parentを動かさずに対象座標に移動可能かどうかを確認
        if (tMaxDistance < tDistance) {
            // 長すぎて届かない
            double tX = Math.cos(tTargetRootAngle) * (this.getLength() * 0.99);
            double tY = Math.sin(tTargetRootAngle) * (this.getLength() * 0.99);
            Point3D tTargetPoint = new Point3D(aPoint.getX() - tX, aPoint.getY() - tY, 0);
            if (!mParent.setEndPoint(tTargetPoint))
                return false;

            return this.setEndPoint(aPoint);
        } else if (tDistance < tMinDistance) {
            // 短すぎて届かない
            double tX = Math.cos(tTargetRootAngle) * this.getLength();
            double tY = Math.sin(tTargetRootAngle) * this.getLength();
            Point3D tTargetPoint = new Point3D(aPoint.getX() + tX, aPoint.getY() + tY, 0);
            if (!mParent.setEndPoint(tTargetPoint))
                return false;

            return this.setEndPoint(aPoint);
        }

        Angle tParentInnerAngle = LineModel.getAngleA(this.getLength(), mParent.getLength(), tDistance);
        Angle tMyAngle = Angle.createAngleAsDegree(180).subtract(LineModel.getAngleA(tDistance, this.getLength(), mParent.getLength()));
        Angle tMyMinusAngle = tMyAngle.minus();
        Angle tSub1 = tMyAngle.subtract(this.getAngle());
        Angle tSub2 = tMyMinusAngle.subtract(this.getAngle());

        //if(tSub1.getDegree() * tSub2.getDegree() < 0){
        //    if(tSub1.getDegree() * this.getAngleVelocity().getDegree() < 0){
        //        tMyAngle = tMyMinusAngle;
        //        tParentInnerAngle = tParentInnerAngle.minus();
        //    }
        //}else
            if(tSub2.abs().getDegree() < tSub1.abs().getDegree()){
            tMyAngle = tMyMinusAngle;
            tParentInnerAngle = tParentInnerAngle.minus();
        }

        Angle tParentAngle = Angle.createAngleAsRad(tTargetParentAngle).subtract(tParentInnerAngle);


        if(!mParent.isRoot())
            tParentAngle = tParentAngle.subtract(mParent.getParent().getAbsoluteAngle());

        mParent.setAngle(tParentAngle);
        this.setAngle(tMyAngle);

        return true;
    }

    public Angle getAbsoluteAngle(){
        if(this.isRoot())
            return mRelativeAngle;
        else
            return mParent.getAbsoluteAngle().add(mRelativeAngle);
    }

    private static Angle getAngleA(double a, double b, double c) {
        double tRadAngle = Math.acos((b * b + c * c - a * a) / (2 * b * c));
        return Angle.createAngleAsRad(tRadAngle);
    }

    private boolean setEndPointAsRoot(Point3D aPoint) {
        double tDistance = aPoint.getDistance(Point3D.origin);
        if (tDistance > 0.1)
            return false;

        double tRadAngle = Math.atan2(aPoint.getY(), aPoint.getX());
        this.setAngle(Angle.createAngleAsRad(tRadAngle));
        return true;
    }

    public LineModel getParent(){
        return mParent;
    }

    public boolean isRoot() {
        return mParent == null;
    }
}
